#ifndef CHASSIS_H
#define CHASSIS_H

#include <cmath>

struct ChassisVelocity
{
    double linear;
    double angular;
};

struct WheelVelocity
{
    double left;
    double right;
};

struct ChassisForceSensorValue
{
    double force;
    double torque;
};

class Chassis
{
private:
    const double l = 470; //mm distance between two drive wheels
    const double reduction_ratio = 70;
    const double wheel_diameter = 150;//mm
    // TODO: the following parameters need to be modified
    const double force_threshold = 5;  //20
    const double force_limit = 100; //100
    const double torque_threshold = 5; //正负x牛米 内不启动  //
    const double torque_limit = 100;
    const double linear_velocity_max = 300;  //mm/s    300
    const double angular_velocity_max = 2;  // rad/s  0.25
    double force_velocity_conversion_coefficient = 0;
    double torque_velocity_conversion_coefficient = 0;

public: // "public" for test
    int ChassisVelocity2WheelVelocity(const ChassisVelocity &chassis_velocity, WheelVelocity &wheel_velocity);
public:
    Chassis();
    int ChassisMoventForceGuide(ChassisForceSensorValue &chassis_force_sensor_value, WheelVelocity &wheel_velocity);
};


#endif // CHASSIS_H
